classdef SLIPcycles < SLIP
    %SLIPcycles uses SLIP methods to simulates distinct phases of gait
    %Version 2 of SLIPcycles 2/19/13
    
    properties
        
    end
    
    methods
        
        function obj=walk_ap2ap(obj)
            obj.sim.vis=0;
            obj=setwalkphases(obj,2);
            obj=setcurrphase(obj,1);
            obj=int_walk_single(obj);
            if ~obj.sim.error
                obj=int_walk_double(obj);
                if ~obj.sim.error
                    obj=setwalkphases(obj,4);
                    obj=setcurrphase(obj,3);
                    obj=int_walk_single(obj);
                end
            end
            obj=data_processing(obj);
        end
        
        function X=walk_ap2ap_optim(obj,aTD)
            obj.param.aTD=aTD;
            obj=setIC(obj,obj.state.IC);
            obj.sim.vis=0;
            obj=setwalkphases(obj,2);
            obj=setcurrphase(obj,1);
            obj=int_walk_single(obj);
            err=100;
            if ~obj.sim.error
                obj=int_walk_double(obj);
                err=10;
                if ~obj.sim.error
                    err=0;
                    obj=setwalkphases(obj,4);
                    obj=setcurrphase(obj,4);
                    obj=int_walk_single(obj);
                end
            end
            X=sum((obj.state.IC(2:4)-obj.state.xe(2:4)).^2)+(obj.sim.error)+err;
        end
        
         % data processing, calculate leg force, leg angle and leg length
        function obj=data_processing(obj)
            %leg length
            phi=NaN(size(obj.env.FP,1),size(obj.state.X,1));
            x=phi;
            l=phi;
            for idx=1:size(obj.env.FP,1)
                l(idx,:)=sqrt(sum((obj.state.X(:,[1 3])-ones(length(obj.state.X),1)*obj.env.FP(idx,1:2)).^2,2));
                x(idx,:)=obj.env.FP(idx,1)-obj.state.X(:,1);
                phi(idx,l(idx,:)<=obj.param.l0)=acosd(x(idx,l(idx,:)<=obj.param.l0)./l(idx,l(idx,:)<=obj.param.l0));
            end
            l=l.*(l<=obj.param.l0)+obj.param.l0.*(l>obj.param.l0);
            obj.state.leg(1,:)=min(l(1:2:end,:),[],1);
            obj.state.leg(2,:)=min(l(2:2:end,:),[],1);
            phi1=phi(1:2:end,:);
            phi2=phi(2:2:end,:);
            obj.state.lphi=NaN(size(obj.state.leg));
            for idx=1:length(obj.state.lphi)
                if sum(isfinite(phi1(:,idx)))>0
                    obj.state.lphi(1,idx)=phi1(isfinite(phi1(:,idx)),idx);
                end
                if sum(isfinite(phi2(:,idx)))>0
                    obj.state.lphi(2,idx)=phi2(isfinite(phi2(:,idx)),idx);
                end
            end
            obj.state.F=obj.param.k.*(obj.param.l0-obj.state.leg);
            obj.state.E.Pot=obj.param.m*obj.param.g*obj.state.X(:,3);
            obj.state.E.Kin=1/2*obj.param.m*sum(obj.state.X(:,2).^2+obj.state.X(:,4).^2,2);
            obj.state.E.spring=0.5*obj.param.k.*(obj.param.l0-obj.state.leg).^2;
            obj.state.E.all=sum(obj.state.E.spring)'+obj.state.E.Pot+obj.state.E.Kin;
        end
    end
end
   